#ifndef __UTILITY_FUNCTIONS__
#define __UTILITY_FUNCTIONS__

#include <iostream>

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
  index j;
  index m = 7 * (p.last_leaf() / 56);
  while (i < m) {
    //index k = i;
    position ak = a + i;
    if(less(*(ak + 1), *(ak + 2))) {
      *(ak) = *(ak + 2);     
      if(less(*(ak + 5), *(ak + 6))) {
	*(ak + 2) = *(ak + 6);
	//i = k + 6;
	//i = (14 * i) - 35 - (42 * (k / 7));	
	i = 49 + 56 * (i / 7);
	//i = 14 * (((k / 7) << 2) + i - 2);
	if(less(*(a + i), *(a + i + 7))) i += 7;
	if(less(*(a + i), x)) {
	  if(less(*(ak + 6), x))
	    if(less(*(ak), x)) {
	      *(ak + 2) =  *(ak);
	      *(ak) = x;
	    }
	    else
	      *(ak + 2) =  x;	
	  else
	    *(ak + 6) = x;
	  return;
	}
	else
	  *(ak + 6) =  *(a + i);
      }
      else {
	*(ak + 2) = *(ak + 5);
	//    i = k + 5;
	//i = (14 * i) - 35 - (42 * (k / 7));	
	i = 35 + 56 * (i / 7);
	//i = 14 * (((k / 7) << 2) + i - 2);
	if(less(*(a + i), *(a + i + 7))) i += 7;
	if(less(*(a + i), x)) {
	  if(less(*(ak + 5), x))
	    if(less(*(ak), x)) {
	      *(ak + 2) =  *(ak);
	      *(ak) = x;
	    }
	    else
	      *(ak + 2) =  x;	
	  else
	    *(ak + 5) = x;
	  return;
	}
	else
	  *(ak + 5) =  *(a + i);
      }	
    }
    else {
      *(ak) = *(ak + 1);     
      if(less(*(ak + 3), *(ak + 4))) {
	*(ak + 1) = *(ak + 4);
	//i = k + 4;
	//i = (14 * i) - 35 - (42 * (k / 7));	
	i = 21 + 56 * (i / 7);
	//i = 14 * (((k / 7) << 2) + i - 2);
	if(less(*(a + i), *(a + i + 7))) i += 7;
	if(less(*(a + i), x)) {
	  if(less(*(ak + 4), x))
	    if(less(*(ak), x)) {
	      *(ak + 1) =  *(ak);
	      *(ak) = x;
	    }
	    else
	      *(ak + 1) =  x;	
	  else
	    *(ak + 4) = x;
	  return;
	}
	else
	  *(ak + 4) =  *(a + i);
      }
      else {
	*(ak + 1) = *(ak + 3);
	//	i = k + 3;
	//i = (14 * i) - 35 - (42 * (k / 7));	
	i = 7 + (i / 7) * 56;
	//i = 14 * (((k / 7) << 2) + i - 2);
	if(less(*(a + i), *(a + i + 7))) i += 7;
	if(less(*(a + i), x)) {
	  if(less(*(ak + 3), x))
	    if(less(*(ak), x)) {
	      *(ak + 1) =  *(ak);
	      *(ak) = x;
	    }
	    else
	      *(ak + 1) =  x;	
	  else
	    *(ak + 3) = x;
	  return;
	}
	else
	  *(ak + 3) =  *(a + i);
      }
    }      
  }

  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
    i = j;
  }
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);	
    else 
      j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      i = j;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  }    
  p.update_all_present(a, i, x, less);
  return;      

  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down2(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
  
  index j;
  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
    i = j;
  }
  
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);	
    else 
      j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      i = j;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  }    
  p.update_all_present(a, i, x, less);
  return;  

  /*  if(p.first_child(i) <= p.last_leaf()) {
    j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      p.update_all_present(a, j, x, less);
      return;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  } 
  else
    p.update_all_present(a, i, x, less);
    return;  */

  
  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

} 

#endif // __UTILITY_FUNCTIONS__


